/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : position_ctrl.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月24日
  ******************************************************************************
  */
/* USER CODE END Header */

#include "control/position_control/position_ctrl.hpp"

#define PID_NUM		6
PID outloop_pid[PID_NUM];
PID *pidU=&outloop_pid[0],*pidV=&outloop_pid[1],*pidW=&outloop_pid[2];
PID *pidX=&outloop_pid[3],*pidY=&outloop_pid[4],*pidZ=&outloop_pid[5];

POSITION_CTRL::POSITION_CTRL()
{
	pidU->Pid_Set_Para(10.0f, 1.0f, 0.5f, 0.5f, 10000, 4*M_PI, M_PI);
	pidV->Pid_Set_Para(10.0f, 1.0f, 0.5f, 0.5f, 10000, 4*M_PI, M_PI);
	pidW->Pid_Set_Para(10.0f, 1.5f, 0.6f, 0.5f, 10000, 4*M_PI, M_PI);
	pidX->Pid_Set_Para(2.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
	pidY->Pid_Set_Para(2.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
	pidZ->Pid_Set_Para(4.0f, 0.05f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);

}

POSITION_CTRL::~POSITION_CTRL()
{

}

void POSITION_CTRL::flight_Mode_Update()
{
	/* 判别飞行模式 ******************************************/
	switch(rcCommand.Key[0])
	{
		case 0: //姿态+定高
			Mode = QAUD_ATTITUDE;
			break;
		case 1: //控速+定高
			Mode = QAUD_VEL_HOLD;
			break;
		case 2: //全自控	//定点+定高
			Mode = QAUD_POS_HOLD;
			break;
	}
}

void POSITION_CTRL::pid_Para_Update()
{
	xQueuePeek(queuePID, &pid_para, 0);
	for(uint8_t i=0;i<PID_NUM;i++)
	{
		outloop_pid[i].Pid_Set_Para(pid_para.kp[i+6], pid_para.ki[i+6], pid_para.kd[i+6]);
	}
}

void POSITION_CTRL::Position_Ctrl_Init()
{
	pid_Para_Update_Time = 0;
	isPosHold = 0;
	isAltHold = 0;
	OneG = 9.788f;
	HOVER_PWM = 710;
	dt = 0.005f;

	osDelay(300);

	xQueuePeek(queuePID, &pid_para, 0);
	for(uint8_t i=0;i<PID_NUM;i++)
	{
		pid_para.kp[i+6] = outloop_pid[i].Pid_Get_Kp();
		pid_para.ki[i+6] = outloop_pid[i].Pid_Get_Ki();
		pid_para.kd[i+6] = outloop_pid[i].Pid_Get_Kd();
	}
	xQueueOverwrite(queuePID,&pid_para);
}


void POSITION_CTRL::Position_Ctrl_Run()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;
	xQueuePeek(queueRCCommand, &rcCommand, 0);
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueFlow, &flow, 0);
	xQueuePeek(queueHeight, &height, 0);
	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
	xQueuePeek(queueUwbPosVel, &ekf_uwb_pos_vel, 0);
	pid_Para_Update_Time++;
	if(pid_Para_Update_Time>10)		//50个控制周期更新一次PID参数
	{
		pid_Para_Update();
		pid_Para_Update_Time = 0;
	}

		flight_Mode_Update();

		if(Mode == QAUD_POS_HOLD)
		{
			float setNEDUvw[2];
			float cosY,sinY;

			sinY = sin(ahrsEuler.Ang[2]);
			cosY = cos(ahrsEuler.Ang[2]);
			//检测是否打杆
			if(rcCommand.Uvw[1]<0.15f&&rcCommand.Uvw[1]>-0.15f&&rcCommand.Uvw[0]<0.15f&&rcCommand.Uvw[0]>-0.15f)		//水平方向如果没有打杆，那么定点
			{
				if(isPosHold == 0)						//由控速模式进入定点模式，取当前的位置作为位置环的设定值
				{
					SetXyz[0] = ekf_uwb_pos_vel.Pos[0];
					SetXyz[1] = ekf_uwb_pos_vel.Pos[1];
				}
				isPosHold = 1;							//定点模式
				FeedBackXyz[0] = ekf_uwb_pos_vel.Pos[0];
				FeedBackXyz[1] = ekf_uwb_pos_vel.Pos[1];



				setNEDUvw[0] = pidX->Pid_Controller(SetXyz[0], FeedBackXyz[0], dt, 1, -5.0f, 5.0f);//x轴速度设定值
				setNEDUvw[1] = pidY->Pid_Controller(SetXyz[1], FeedBackXyz[1], dt, 1, -5.0f, 5.0f);//y轴速度设定值

				SetUvw[0] =  setNEDUvw[0]*cosY + setNEDUvw[1]*sinY;			//NED坐标系速度转机头坐标系速度
				SetUvw[1] = -setNEDUvw[0]*sinY + setNEDUvw[1]*cosY;

			}
			else
			{
				isPosHold = 0;							//否则检测到遥控器打杆，控速模式
				SetUvw[0] = rcCommand.Uvw[0];//x轴速度设定值
				SetUvw[1] = rcCommand.Uvw[1];//y轴速度设定值
			}

			//检测Z轴是否打杆
			if(rcCommand.Uvw[2]>-0.1f&&rcCommand.Uvw[2]<0.1f)		// 如果没有打杆，那么进入定高模式
			{
				if(isAltHold == 0)
				{
					SetXyz[2] = -height.height;
					SetXyz[2] = fConstrain(SetXyz[2], -2.5f, 0.0f);
				}

				isAltHold = 1;
				FeedBackXyz[2] = -height.height;
				SetUvw[2] = pidZ->Pid_Controller(SetXyz[2],FeedBackXyz[2],dt,0);

			}
			else	//否则Z轴控速度，速度范围在-2.5m/s~2.5m/s,由遥控器给定期望速度
			{
				isAltHold = 0;
				SetUvw[2] = rcCommand.Uvw[2];

			}

			FeedBackUvw[0] = flow.flowx;
			FeedBackUvw[1] = flow.flowy;
			FeedBackUvw[2] = -height.speedZ;


			pitch_command = -pidU->Pid_Controller(SetUvw[0], FeedBackUvw[0], dt, 1,-30.0f,30.0f);
			roll_command = pidV->Pid_Controller(SetUvw[1], FeedBackUvw[1], dt, 1,-30.0f,30.0f);
			SetAcc[2] = pidW->Pid_Controller(SetUvw[2], FeedBackUvw[2], dt, 1,-9.788f,8.0f);

			float cosR,cosP;
			cosR = cos(ahrsEuler.Ang[0]);
			cosP = cos(ahrsEuler.Ang[1]);
			thrust_command = -(SetAcc[2] - OneG)/(cosP*cosR);
			Z_OUT = HOVER_PWM*sqrt(thrust_command/OneG);
			inloop_control.thrust_PWM = Z_OUT;

			inloop_control.setPosition[0] = SetXyz[0];
			inloop_control.setPosition[1] = SetXyz[1];
			inloop_control.setPosition[2] = -SetXyz[2];
			inloop_control.setSpeed[0] = SetUvw[0];
			inloop_control.setSpeed[1] = SetUvw[1];
			inloop_control.setSpeed[2] = -SetUvw[2];

			inloop_control.setAng[0] = roll_command;
			inloop_control.setAng[1] = pitch_command;
		}

		else if(Mode == QAUD_VEL_HOLD)
		{
			//检测是否打杆
			if(rcCommand.Uvw[1]<0.15f&&rcCommand.Uvw[1]>-0.15f&&rcCommand.Uvw[0]<0.15f&&rcCommand.Uvw[0]>-0.15f)		//水平方向如果没有打杆，那么速度控为0，相当于光流定点
			{
				SetUvw[0] = 0.0f;//x轴速度设定值
				SetUvw[1] = 0.0f;//y轴速度设定值
			}
			else
			{
				SetUvw[0] = rcCommand.Uvw[0];//x轴速度设定值
				SetUvw[1] = rcCommand.Uvw[1];//y轴速度设定值
			}

			//检测Z轴是否打杆
			if(rcCommand.Uvw[2]>-0.1f&&rcCommand.Uvw[2]<0.1f)		// 如果没有打杆，那么进入定高模式
			{
				if(isAltHold == 0)
				{
					SetXyz[2] = -height.height;
					SetXyz[2] = fConstrain(SetXyz[2], -2.5f, 0.0f);
				}

				isAltHold = 1;
				FeedBackXyz[2] = -height.height;
				SetUvw[2] = pidZ->Pid_Controller(SetXyz[2],FeedBackXyz[2],dt,0);

			}
			else	//否则Z轴控速度，速度范围在-2.5m/s~2.5m/s,由遥控器给定期望速度
			{
				isAltHold = 0;
				SetUvw[2] = rcCommand.Uvw[2];

			}
			FeedBackUvw[0] = flow.flowx;
			FeedBackUvw[1] = flow.flowy;
			FeedBackUvw[2] = -height.speedZ;


			pitch_command = -pidU->Pid_Controller(SetUvw[0], FeedBackUvw[0], dt, 1,-30.0f,30.0f);
			roll_command = pidV->Pid_Controller(SetUvw[1], FeedBackUvw[1], dt, 1,-30.0f,30.0f);
			SetAcc[2] = pidW->Pid_Controller(SetUvw[2], FeedBackUvw[2], dt, 1,-9.788f,8.0f);

			float cosR,cosP;
			cosR = cos(ahrsEuler.Ang[0]);
			cosP = cos(ahrsEuler.Ang[1]);

			thrust_command = -(SetAcc[2] - OneG)/(cosP*cosR);



			Z_OUT = HOVER_PWM*sqrt(thrust_command/OneG);

			inloop_control.thrust_PWM = Z_OUT;

			inloop_control.setPosition[0] = SetXyz[0];
			inloop_control.setPosition[1] = SetXyz[1];
			inloop_control.setPosition[2] = -SetXyz[2];
			inloop_control.setSpeed[0] = SetUvw[0];
			inloop_control.setSpeed[1] = SetUvw[1];
			inloop_control.setSpeed[2] = -SetUvw[2];

			inloop_control.setAng[0] = roll_command;
			inloop_control.setAng[1] = pitch_command;
		}


		else if(Mode == QAUD_ATTITUDE)
		{
			inloop_control.setAng[0] = rcCommand.Ang[0];
			inloop_control.setAng[1] = rcCommand.Ang[1];



			Z_OUT = rcCommand.Thr;

			inloop_control.thrust_PWM = Z_OUT;

			inloop_control.setPosition[2] = -SetXyz[2];
			inloop_control.setSpeed[2] = -SetUvw[2];



		}

	xQueueOverwrite(queueInloopControl,&inloop_control);

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;

}

POSITION_CTRL positionCtrl;

extern "C" void positionCtrl_main(void *argument)
{
	positionCtrl.Position_Ctrl_Init();
	for(;;)
	{
		osSemaphoreAcquire(semPositionCtrl,0xffffffff);
		positionCtrl.Position_Ctrl_Run();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
